#include "Robot2489.h"
#include "WPILib.h"

void Robot2489::UpdateSpeeds(void){
	

	double crouch;

	//Enable crouch for left or right side
	crouch = (1 - rightStick->GetZ()) / 2;
		
	//Update wheel speeds, scaling into a logistic progression and
	//decreasing if crouch is enabled
	leftValue  = LogisticSpeedProgression(leftStick ->GetY())* crouch ;
	rightValue = LogisticSpeedProgression(rightStick->GetY())* crouch ;

	if(leftStick->GetTrigger() || rightStick->GetTrigger()){
	    leftValue  = (leftValue + rightValue) / 2;
            rightValue = leftValue;
        }            
	
	//Update turret wheel speeds
	if (bottomWheelValue > 1 && gameStick->GetRawAxis(1) < 0){
		bottomWheelValue = bottomWheelValue + 0.0005*LogisticSpeedProgression(gameStick->GetRawAxis(1));
	}
	else if (bottomWheelValue < -1 && gameStick->GetRawAxis(1) > 1){
		bottomWheelValue = bottomWheelValue + 0.0005*LogisticSpeedProgression(gameStick->GetRawAxis(1));
	}
	else if (-1 < bottomWheelValue && bottomWheelValue < 1){
		bottomWheelValue = bottomWheelValue + 0.0005*LogisticSpeedProgression(gameStick->GetRawAxis(1));
	}
	if (fabs(enhancedIO->GetAnalogIn(2) - potValue) > 0.01){
		potValue = enhancedIO->GetAnalogIn(2);
		bottomWheelValue = (potValue + 1.067) / 4.267;
	}
	
	//Check whether button is currently being pressed, so that speed isn't continuously updated
	
	//Sticky crouch

}

//Return a value from -1 to 1 that's scaled into a logistic progression.
double Robot2489::LogisticSpeedProgression(double jpos)
{
	double motorspeed; // define return value variable
	
	if (jpos >= -0.06 && jpos <= 0.06) // define dead zone
		return 0.0;
	
	// function for positive values
	if  (jpos > 0) motorspeed =  1.0 / (1.0 + 250.0 * std::exp(-10.0 * jpos));
	// function for negative values}
	else           motorspeed = -1.0 / (1.0 + 250.0 * std::exp( 10.0 * jpos));
	
	if (motorspeed >= 1.0) // make sure return value doesn't exceed 1.0
		motorspeed = 1.0;	
	return motorspeed;
}

//Control the ramp tool (button 7 to move it down, button 6 to return it to
//rest position).
void Robot2489::ControlRampTool(void)
{
	//reverse = down, forward = up
	//rampToolStop->Get = true when off, false when on
	if (manipStick->GetRawButton(4)){
		rampTool->Set(Relay::kReverse);
		previousRampToolDirection = Relay::kReverse;
	}
	else if (!manipStick->GetRawButton(4) && (previousRampToolDirection == Relay::kReverse)){
		rampTool->Set(Relay::kOff);
	}
	if (manipStick->GetRawButton(2) && rampToolStop->Get()){
		rampTool->Set(Relay::kForward);
		previousRampToolDirection = Relay::kForward;
	}
	else if (!rampToolStop->Get() && (previousRampToolDirection != Relay::kReverse)){
		rampTool->Set(Relay::kOff);
	}
	
}

void Robot2489::TurnAuton(double theta){
	leftEncoder->Reset();
	rightEncoder->Reset();
	
	int sign = (int) (fabs(theta)/theta);
	
	rightValue = sign * 0.2;
	leftValue = -sign * 0.2;
	
	myRobot->TankDrive(-leftValue, -rightValue);
	Wait(0.005);
	
	while (IsAutonomous() && 
		   leftEncoder ->GetDistance() < -theta*ROBO_WIDTH/2 &&
		   rightEncoder->GetDistance() <  theta*ROBO_WIDTH/2 )
	{
		CorrectDrive();
		myRobot->TankDrive(-correctedLeftValue, -correctedRightValue);
		Wait(0.005);
	}
	/*
	leftEncoder->SetPIDSourceParameter(Encoder::kDistance);
	rightEncoder->SetPIDSourceParameter(Encoder::kDistance);
	leftMotorController->SetTolerance(0.03);
	leftMotorController->SetSetpoint(-theta*ROBO_WIDTH/2);
	rightMotorController->SetSetpoint(theta*ROBO_WIDTH/2);
	*/
}

void Robot2489::MoveAuton(double distance){
	leftEncoder ->Reset();
	rightEncoder->Reset();
	leftEncoder->SetPIDSourceParameter(Encoder::kDistance);
	rightEncoder->SetPIDSourceParameter(Encoder::kDistance);
	leftMotorController ->SetTolerance(0.01);
	rightMotorController->SetTolerance(0.01);
	leftMotorController ->SetContinuous();
	rightMotorController->SetContinuous();
	leftMotorController->SetInputRange(-1.0*fabs(distance) - 10, fabs(distance) + 10);
	rightMotorController->SetInputRange(-1.0*fabs(distance) - 10, fabs(distance) + 10);
	leftMotorController->SetOutputRange(-0.2, 0.2);
	rightMotorController->SetOutputRange(-0.2, 0.2);
	leftMotorController ->SetSetpoint(distance);
	rightMotorController->SetSetpoint(distance);
	rightMotorController->Enable();
	leftMotorController->Enable();
}
/*
void Robot2489::TimeMove(double time){
	autoTimer->Start();
	while(autoTimer->Get()<time){
		leftMotor->Set(1.0);
		rightMotor->Set(1.0);
	}
	leftMotor->Set(0);
	rightMotor->Set(0);
	autoTimer->Stop();
	autoTimer->Reset();
}
void Robot2489::TimeTurn(double time){
	autoTimer->Start();
	while(autoTimer->Get()<time){
		leftMotor->Set(-1.0);
		rightMotor->Set(1.0);
	}
	leftMotor->Set(0);
	rightMotor->Set(0);
	autoTimer->Stop();
	autoTimer->Reset();
}
*/
void Robot2489::CorrectDrive(){
	double needed_ratio = fabs(leftValue/rightValue);
	double actual_ratio = fabs(leftEncoder->GetRaw()/rightEncoder->GetRaw());
	if (needed_ratio > (actual_ratio - MAX_ERROR)){
		correctedRightValue = correctedRightValue * (actual_ratio / needed_ratio);
	}
	else if (needed_ratio < (actual_ratio + MAX_ERROR)){
		correctedLeftValue = correctedLeftValue * (needed_ratio / actual_ratio);
	}
}

void Robot2489::ControlIntake(){
#ifdef INTAKE_CONTROL
	bool sensorTopPresent;
	bool sensorBotPresent;
	sensorTopPresent = !proximtySensorTop->Get();
	sensorBotPresent = !proximtySensorBottom->Get();
	
	
	
	if(sensorTopPresent){
		solenoidTopOn->Set(1);
		solenoidTopOff->Set(0);
	} else{
		solenoidTopOn->Set(0);
		solenoidTopOff->Set(1);
	}
	
	if(sensorBotPresent){
		solenoidBottomOn->Set(1);
		solenoidBottomOff->Set(0);
	} else{
		solenoidBottomOn->Set(0);
		solenoidBottomOff->Set(1);
	}
#endif
}
